#include "sensor/mpu6050.h"
#include "board.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include <math.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define Q30            1073741824.0f
#define DEFAULT_MPU_HZ (100)

mpu6050_t* gp_mpu6050;
static signed char gyro_orientation[9] = { -1, 0, 0, 0, -1, 0, 0, 0, 1 };

int motion_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char* data)
{
	return gp_mpu6050->ops.i2c_write(slave_addr, &reg_addr, data, length);
}

int motion_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char* data)
{
	return gp_mpu6050->ops.i2c_read(slave_addr, &reg_addr, data, length);
}

void motion_delay_ms(int num_ms)
{
	delay_ms(num_ms);
}

void motion_get_ms(uint32_t* ms)
{
	*ms = board_get_ms();
}

static inline unsigned short inv_row_2_scale(const signed char* row)
{
	unsigned short b;

	if (row[0] > 0)
		b = 0;
	else if (row[0] < 0)
		b = 4;
	else if (row[1] > 0)
		b = 1;
	else if (row[1] < 0)
		b = 5;
	else if (row[2] > 0)
		b = 2;
	else if (row[2] < 0)
		b = 6;
	else
		b = 7; // error
	return b;
}

static inline unsigned short inv_orientation_matrix_to_scalar(const signed char* mtx)
{
	unsigned short scalar;

	/*
	   XYZ  010_001_000 Identity Matrix
	   XZY  001_010_000
	   YXZ  010_000_001
	   YZX  000_010_001
	   ZXY  001_000_010
	   ZYX  000_001_010
	 */

	scalar = inv_row_2_scale(mtx);
	scalar |= inv_row_2_scale(mtx + 3) << 3;
	scalar |= inv_row_2_scale(mtx + 6) << 6;

	return scalar;
}

static inline int run_self_test(void)
{
	int ret;
	long gyro[3], accel[3];
	unsigned char i = 0;

	ret = mpu_run_self_test(gyro, accel);
	if (ret == 0x7) {
		/* Test passed. We can trust the gyro data here, so let's push it down
		 * to the DMP.
		 */
		for (i = 0; i < 3; i++) {
			gyro[i] = (long)(gyro[i] * 32.8f); // convert to +-1000dps
			accel[i] *= 2048.f;                // convert to +-16G
			accel[i] = accel[i] >> 16;
			gyro[i] = (long)(gyro[i] >> 16);
		}

		mpu_set_gyro_bias_reg(gyro);
		mpu_set_accel_bias_6050_reg(accel);

		return 0;
	}

	return -1;
}

int mpu6050_init(mpu6050_t* sensor)
{
	int ret;
	struct int_param_s int_param;

	gp_mpu6050 = sensor;

	ret = mpu_init(&int_param);
	if (ret) {
		return -1;
	}
	printf("success: mpu_init()\r\n");

	ret = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
	if (ret) {
		return -2;
	}
	printf("success: mpu_set_sensors()\r\n");

	ret = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
	if (ret) {
		return -3;
	}
	printf("success: mpu_configure_fifo()\r\n");

	ret = mpu_set_sample_rate(DEFAULT_MPU_HZ);
	if (ret) {
		return -4;
	}
	printf("success: mpu_set_sample_rate()\r\n");

	ret = dmp_load_motion_driver_firmware();
	if (ret) {
		return -5;
	}
	printf("success: dmp_load_motion_driver_firmware()\r\n");

	ret = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
	if (ret) {
		return -6;
	}
	printf("success: dmp_set_orientation()\r\n");

	ret = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT
	    | DMP_FEATURE_SEND_RAW_ACCEL
	    | DMP_FEATURE_SEND_CAL_GYRO
	    | DMP_FEATURE_GYRO_CAL);
	if (ret) {
		return -7;
	}
	printf("success: dmp_enable_feature()\r\n");

	ret = dmp_set_fifo_rate(DEFAULT_MPU_HZ);
	if (ret) {
		return -8;
	}
	printf("success: dmp_set_fifo_rate()\r\n");

	ret = run_self_test();
	if (ret) {
		return -9;
	}
	printf("success: run_self_test()\r\n");

	ret = mpu_set_dmp_state(1);
	if (ret) {
		return -10;
	}
	printf("success: mpu_set_dmp_state()\r\n");

	return 0;
}

int mpu6050_read(float* pitch, float* roll, float* yaw)
{
	int ret = 0;
	float q0 = 0.0f;
	float q1 = 0.0f;
	float q2 = 0.0f;
	float q3 = 0.0f;
	short gyro[3];
	short accel[3];
	short sensors;
	unsigned char more;
	long quat[4];
	uint32_t timestamp;

	ret = dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more);
	if (ret) {
		return -1;
	}

	if (sensors & INV_WXYZ_QUAT) {
		q0 = quat[0] / Q30;
		q1 = quat[1] / Q30;
		q2 = quat[2] / Q30;
		q3 = quat[3] / Q30;

		*pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
		*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;
		*yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;
	} else {
		return -2;
	}

	return 0;
}
